/*
 * VirtualPosition2d.java
 *
 * Created on 15 de noviembre de 2007, 01:08 PM
 *
 * To change this template, choose Tools | Template Manager
 * and open the template in the editor.
 */

package Robot;


import Utils.*;
/**
 *
 * @author nxbot
 */
public class VirtualPosition2d extends Position2d {
    
    private float linearVel, angleVel;
    private Pose2d initPos;
    private double angle, x, y;
    private long lastTime;
    private double TIME_CONVERSION = 0.000000001;
    private boolean firstTime = true;
    private boolean isDebugging = false;
    
    /** Creates a new instance of VirtualPosition2d */
    public VirtualPosition2d(Robot robot, String name, int index) {
        
        super(robot, name, index);
        lastTime = 0;
        data=new Pose2d(0f,0f,0f);
        initPos = new Pose2d(0f,0f,0f);
        
    }
    /**
     *
     * @return
     */
    public boolean update(){
        
        long actualTime = System.nanoTime();
        
        if(firstTime)
            firstTime = false;
        else
        {
            //System.out.println(initPos.getPa());
            double angle = getAngle(actualTime - lastTime);
            double x = getX(actualTime - lastTime);
            double y = getY(actualTime - lastTime);
            
            
            

            data.setPa((float)angle);
            data.setPx((float)x);
            data.setPy((float)y);
            
            data.add(initPos);
            
            data.setPa((float)Vector2d.angleCorrMPI2PI(data.getPa()));
            //data.add(initialPosition);

            if(isDebugging){
                System.out.println("VIRTUALPOSITION2D\nPose of Virtual robot " +Integer.toString(robot.getIndex()));
                System.out.println("X: " +Double.toString(data.getPx())+" Y: "+Double.toString(data.getPy())+" Theta: "+Double.toString(data.getPa()));
                
            }
            
        }
        
        lastTime = actualTime;
        return true;
        
        
        
    
    }
    

    /**
     *
     * @param initial
     */
     public void setPosition(Pose2d initial){
        //data.copy(initial);
        initial.copy(initPos);
     }

    /**
     *
     * @param interval
     * @return
     */
    private double getAngle(long interval){
        
         angle += angleVel * interval * TIME_CONVERSION;
         
         if(angle>Math.PI*2)
             angle -= Math.PI*2;
         else if(angle>Math.PI)
             angle -= Math.PI*2;
         //System.out.println(angle);
         return angle;
    
    }

    /**
     *
     * @param interval
     * @return
     */
    private double getX(long interval){
        
               
         x += linearVel * Math.cos(data.getPa()) * interval *TIME_CONVERSION;
         //System.out.println("Pos X:"+Double.toString(x));
         //x *= TIME_CONVERSION;
         
         return x;
    
    }
    
    private double getY(long interval){
        
         y += linearVel * Math.sin(data.getPa()) * interval * TIME_CONVERSION;
         
         return y;
    
    }
    
    /*
     *Virtual device is always ready
     */
    public boolean isReady()
    {
       return true;
    }
    
    
     
    public void setSpeed(float val1, float val2){
        
        //System.out.println("VIRTUALPOSITION2D\nLinearVel: "+val1+"angularVel: "+val2);
        update();
        linearVel = val1;
        angleVel = val2;
        //System.out.println(linearVel);
        //System.out.println(angleVel);
        
        
    }
    
    
    public void setDebugMode(boolean state)
    {
        this.isDebugging = state;
    
    }
    
}
